#include "camera_topic.h"

using namespace std;
using namespace cv;
using namespace cameras;

void CameraTopic::get_param()
{
	nh_private_.getParam("camera_state", camera_state_);
	nh_private_.getParam("camera_id", camera_id_);
	nh_private_.getParam("camera_rate", camera_rate_);
	nh_private_.getParam("camera_width", camera_width_);
	nh_private_.getParam("camera_height", camera_height_);
    nh_private_.getParam("camera_topic", camera_topic_);
    nh_private_.getParam("camera_calibration_file", camera_calibration_file_);
	nh_private_.getParam("cameras_control_topic", cameras_control_topic_);

    nh_private_.getParam("front_lower_camera_id", front_lower_camera_id_);
	nh_private_.getParam("front_lower_camera_topic", front_lower_camera_topic_);
    nh_private_.getParam("front_lower_camera_calibration_file", front_lower_camera_calibration_file_);
}

void CameraTopic::camera_state_sub(const std_msgs::UInt32 msg)
{
	unsigned int state_data = msg.data;
    switch(camera_id_) {
        case FRONT_UPPER_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>FRONT_UPPER_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case FRONT_LOWER_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>FRONT_LOWER_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case FRONT_LEFT_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>FRONT_LEFT_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case FRONT_RIGHT_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>FRONT_RIGHT_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case REAR_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>REAR_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case REAR_LEFT_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>REAR_LEFT_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case REAR_RIGHT_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>REAR_RIGHT_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
		case TRAFFIC_LIGHT_CAMERA:
            camera_mutex_.lock();
			camera_state_ = ((state_data>>TRAFFIC_LIGHT_CAMERA)%2 == 1) ? true:false;
            camera_mutex_.unlock();
            break;
        default:
            break;
    }
    return;
}

void CameraTopic::upper_lower_camera()
{
    cv::VideoCapture capture_upper, capture_lower;
    try {
        if(!capture_upper.open(camera_id_)) {
            ROS_ERROR("open camera %d error", camera_id_);
        }
        capture_upper.set(CV_CAP_PROP_FRAME_WIDTH, camera_width_);
        capture_upper.set(CV_CAP_PROP_FRAME_HEIGHT, camera_height_);
        capture_upper.set(CV_CAP_PROP_FPS, camera_rate_);

        if(!capture_lower.open(front_lower_camera_id_)) {
            ROS_ERROR("open camera %d error", front_lower_camera_id_);
        }
        capture_lower.set(CV_CAP_PROP_FRAME_WIDTH, camera_width_);
        capture_lower.set(CV_CAP_PROP_FRAME_HEIGHT, camera_height_);
        capture_lower.set(CV_CAP_PROP_FPS, camera_rate_);
    } catch (const std::exception& e) {
        ROS_ERROR("camera open camera: %s", e.what());
    }

    ROS_WARN("camera %d is run", camera_id_);
    image_transport::ImageTransport camera_image_transport(nh_private_);
    image_transport::Publisher publish_image = camera_image_transport.advertise(camera_topic_, 1);
    image_transport::Publisher publish_image_calib = camera_image_transport.advertise(camera_topic_+"_calib", 1);
    image_transport::Publisher publish_front_lower_image = camera_image_transport.advertise(front_lower_camera_topic_, 1);
    image_transport::Publisher publish_front_lower_image_calib = camera_image_transport.advertise(front_lower_camera_topic_+"_calib", 1);
    sensor_msgs::ImagePtr msg_image, msg_image_calib, msg_front_lower_image, msg_front_lower_image_calib;
    
    int upper_count = 0, lower_count = 0;
    cv::Mat image_src, image_src_calib, front_lower_image_src, front_lower_image_src_calib;

    while(ros::ok()) {
        try {
            camera_mutex_.lock();
            state_update_ = camera_state_;
            camera_mutex_.unlock();
        } catch (const std::exception& e) {
            ROS_ERROR("camera_mutex_: %s", e.what());
        }

        if(!capture_upper.grab()) {
			ROS_ERROR("camera %d grab error", camera_id_);
			usleep(sleep_time_);
			continue;
		}
        if(!capture_lower.grab()) {
			ROS_ERROR("camera %d grab error", front_lower_camera_id_);
			usleep(sleep_time_);
			continue;
		}

        if(state_update_ == false) {
			usleep(sleep_time_);
            //ROS_WARN("camera_state_ %d false", camera_id_);
			continue;
		}

        if(!capture_upper.retrieve(image_src)) {
            ROS_ERROR("camera %d retrieve error", camera_id_);
			usleep(sleep_time_);
            continue;
        }
        if(!capture_lower.retrieve(front_lower_image_src)) {
            ROS_ERROR("camera %d retrieve error", front_lower_camera_id_);
			usleep(sleep_time_);
            continue;
        }

        image_sum_ = cv::sum(image_src(cv::Rect(0, 0, image_src.rows/4, image_src.cols/4)));
        if(image_sum_[0] < 1) {
			usleep(sleep_time_);
            upper_count++;
            if(upper_count < img_sum_count_)
                continue;
			ROS_ERROR("camera %d image_sum_: %f, get frame error", camera_id_, 1.0*image_sum_[0]);
            return;
        }
        upper_count = 0;

        image_sum_ = cv::sum(front_lower_image_src(cv::Rect(0, 0, front_lower_image_src.rows/4, front_lower_image_src.cols/4)));
        if(image_sum_[0] < 1) {
			usleep(sleep_time_);
            lower_count++;
            if(lower_count < img_sum_count_)
                continue;
			ROS_ERROR("camera %d image_sum_: %f, get frame error", front_lower_camera_id_, 1.0*image_sum_[0]);
            return;
        }
        lower_count = 0;

        remap(image_src, image_src_calib, camera_map_pre_, camera_map_post_, cv::INTER_LINEAR);
        remap(front_lower_image_src, front_lower_image_src_calib, front_lower_camera_map_pre_, front_lower_camera_map_post_, cv::INTER_LINEAR);

        //发布图像
        msg_image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src).toImageMsg();
        publish_image.publish(msg_image);
        msg_image_calib = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src_calib).toImageMsg();
        publish_image_calib.publish(msg_image_calib);

        msg_front_lower_image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", front_lower_image_src).toImageMsg();
        publish_front_lower_image.publish(msg_front_lower_image);
        msg_front_lower_image_calib = cv_bridge::CvImage(std_msgs::Header(), "bgr8", front_lower_image_src_calib).toImageMsg();
        publish_front_lower_image_calib.publish(msg_front_lower_image_calib);
    }
    return;
}

void CameraTopic::one_camera()
{
    cv::VideoCapture capture;
    try {
        if(!capture.open(camera_id_)) {
            ROS_ERROR("open camera %d error", camera_id_);
        }
        capture.set(CV_CAP_PROP_FRAME_WIDTH, camera_width_);
        capture.set(CV_CAP_PROP_FRAME_HEIGHT, camera_height_);
        capture.set(CV_CAP_PROP_FPS, camera_rate_);
    } catch (const std::exception& e) {
        ROS_ERROR("camera open camera: %s", e.what());
    }

    ROS_WARN("camera %d is run", camera_id_);
    image_transport::ImageTransport camera_image_transport(nh_private_);
    image_transport::Publisher publish_image = camera_image_transport.advertise(camera_topic_, 1);
    image_transport::Publisher publish_image_calib = camera_image_transport.advertise(camera_topic_+"_calib", 1);
    sensor_msgs::ImagePtr msg_image, msg_image_calib;

    int count = 0;
    cv::Mat image_src, image_src_calib;

    while(ros::ok()) {
        try {
            camera_mutex_.lock();
            state_update_ = camera_state_;
            camera_mutex_.unlock();
        } catch (const std::exception& e) {
            ROS_ERROR("camera_mutex_: %s", e.what());
        }

        if(!capture.grab()) {
			ROS_ERROR("camera %d grab error", camera_id_);
			usleep(sleep_time_);
			continue;
		}

        if(state_update_ == false) {
			usleep(sleep_time_);
            //ROS_WARN("camera_state_ %d false", camera_id_);
			continue;
		}

        if(!capture.retrieve(image_src)) {
            ROS_ERROR("camera %d retrieve error", camera_id_);
			usleep(sleep_time_);
            continue;
        }

        image_sum_ = cv::sum(image_src(cv::Rect(0, 0, image_src.rows/4, image_src.cols/4)));
        if(image_sum_[0] < 1) {
			usleep(sleep_time_);
            count++;
            if(count < img_sum_count_)
                continue;
			ROS_ERROR("camera %d image_sum_: %f, get frame error", camera_id_, 1.0*image_sum_[0]);
            return;
        }
        count = 0;
        remap(image_src, image_src_calib, camera_map_pre_, camera_map_post_, cv::INTER_LINEAR);

        //发布图像
        msg_image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src).toImageMsg();
        publish_image.publish(msg_image);
        msg_image_calib = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src_calib).toImageMsg();
        publish_image_calib.publish(msg_image_calib);
    }
    return;
}

void CameraTopic::save_img(cv::Mat& image, std::string& dir_path)
{
    std::vector<int> params, params_png;
    params.resize(3, 0);
    params[0] = cv::IMWRITE_JPEG_QUALITY;
    params[1] = 95; //0-100

    time_t cap_time = time(0);
    char ch_time[64];
    strftime(ch_time, sizeof(ch_time), "%Y%m%d_%H%M%S", localtime(&cap_time));

    cv::imwrite(dir_path+ch_time+".jpg", image, params);
}

void CameraTopic::run()
{
    if(camera_id_ == FRONT_UPPER_CAMERA) {
        upper_lower_camera();
    }
    else {
        one_camera();
    }
    return;
}

void CameraTopic::init_calibration_para()
{
	cv::Mat camera_instrinsics_, front_lower_camera_instrinsics_;
	cv::Mat camera_distortion_coefficients_, front_lower_camera_distortion_coefficients_;
    cv::Size camera_map_size(1280,720);

    cv::FileStorage camera_calibration_fs(camera_calibration_file_, cv::FileStorage::READ);
    cv::FileStorage front_lower_camera_calibration_fs(front_lower_camera_calibration_file_, cv::FileStorage::READ);

    if (!camera_calibration_fs.isOpened()) {
		ROS_ERROR("camera_calibration_file_ %s open falied", camera_calibration_file_);
    }

    if (!front_lower_camera_calibration_fs.isOpened()) {
		ROS_ERROR("front_lower_camera_calibration_file_ %s open falied", front_lower_camera_calibration_file_);
    }

    camera_calibration_fs["CameraMat"] >> camera_instrinsics_;
    camera_calibration_fs["DistCoeff"] >> camera_distortion_coefficients_;
    camera_calibration_fs.release();

    front_lower_camera_calibration_fs["CameraMat"] >> front_lower_camera_instrinsics_;
    front_lower_camera_calibration_fs["DistCoeff"] >> front_lower_camera_distortion_coefficients_;
    front_lower_camera_calibration_fs.release();

	cv::initUndistortRectifyMap(camera_instrinsics_, camera_distortion_coefficients_,  Mat(), 
                                getOptimalNewCameraMatrix(camera_instrinsics_, camera_distortion_coefficients_, 
                                camera_map_size, 0, camera_map_size, 0), 
                                camera_map_size, 0, camera_map_pre_, camera_map_post_);

	cv::initUndistortRectifyMap(front_lower_camera_instrinsics_, front_lower_camera_distortion_coefficients_, Mat(), 
                                getOptimalNewCameraMatrix(front_lower_camera_instrinsics_,  
                                front_lower_camera_distortion_coefficients_, 
                                camera_map_size, 0, camera_map_size, 0), 
                                camera_map_size, 0, front_lower_camera_map_pre_, front_lower_camera_map_post_);
}

CameraTopic::CameraTopic(ros::NodeHandle& private_node)
    : nh_private_(private_node)
{
	get_param();
    sub_private_ = nh_private_.subscribe<std_msgs::UInt32>(cameras_control_topic_, 1, &CameraTopic::camera_state_sub, this);
    //sub_private_ = nh_private_.subscribe<std_msgs::UInt32>(cameras_control_topic_, 1, boost::bind(&CameraTopic::camera_state_sub, this, _1));
    init_calibration_para();
}

CameraTopic::~CameraTopic()
{

}
